/* clock em Hz */
#define F_CPU 16000000UL

#include <stdio.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#include "rot_tsetups.h"
#include "rot_servo.h"
#include "rot_serial.h"
#include "rot_adc.h"
#include "rot_tacho.h"

#define MANUAL 0
#define AUTO 1

void check_mode(uint8_t *mode) {

  static int last_ppm;

  if ( (last_ppm>3000) && (ppm_vals[MODE_CH]>3000) ) {
    *mode = AUTO;
  } else {
    *mode = MANUAL;
  }
  
  last_ppm = ppm_vals[MODE_CH];

}

void steps_servo() {

  int pos;

  if (SERVO_POS_CNTS < 93) {
    pos = SERVO_MAX-100;
  } else {
    pos = SERVO_MIN+300;
  }

  MOVE_SERVO_TO(pos);

}

void main_init(void) {

  timer1_init();
  timer2_init();
  serial_init((F_CPU/SERIAL_BAUD/16)-1);
  servo_init();
  adc_init();
  tacho_init();

}

int main(void) {

  char msg[100];
  uint8_t cur_mode=MANUAL;
  uint8_t steps=0;
  int pos=0, adc_val;
  int rpm;

  main_init();
  /* Enable Interrupts */
  sei();
  
  while (1){
    if (TASK1_FLAG) {
      check_mode(&cur_mode);
      if (cur_mode == MANUAL) {
	pos = ppm_vals[MOTOR_CH];
	MOVE_SERVO_TO(pos);
	steps = 0;
      } else if (cur_mode == AUTO) {
	steps = 1;
      } else {
	pos = SERVO_MIN;
	MOVE_SERVO_TO(pos);
	steps = 0;
      }
      TASK1_FLAG = 0;
    }

    if (TASK2_FLAG) {
      if (steps) {
	steps_servo();
      }
      adc_val = adc_read(PC0);
      rpm = tacho_get_rpm();
      sprintf(msg,"V\tMODE=%u,POS=%d,FOR=%d,RPM=%d,nb=%u\n",
	      cur_mode,pos,adc_val,rpm,tacho_nb_pulse);
      serial_transmit(msg);
      TASK2_FLAG = 0;
    }
  }
  return 0;
};
